{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "  # We must install required packages if we are in Google Colab\n",
    "  import google.colab\n",
    "  %pip install roboticstoolbox-python>=1.0.2\n",
    "except:\n",
    "  # We are not in Google Colab\n",
    "  # Apply custon style to notebook\n",
    "  from IPython.core.display import HTML\n",
    "  import pathlib\n",
    "  styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n",
    "  styles = open(styles_path, \"r\").read()\n",
    "  HTML(f\"<style>{styles}</style>\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 2.0 The Manipulator Jacobian\n",
    "\n",
    "<!-- #### Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications -->\n",
    "\n",
    "<!-- _By Jesse Haviland and Peter Corke_ -->\n",
    "$\\large{\\text{Manipulator Differential Kinematics}} \\\\ \\large{\\text{Part I: Kinematics, Velocity, and Applications}}$\n",
    "\n",
    "$\\text{By Jesse Haviland and Peter Corke}$\n",
    "\n",
    "<br>\n",
    "\n",
    "The sections of the Tutorial paper related to this notebook are listed next to each contents entry.\n",
    "It is beneficial to read these sections of the paper before attempting the notebook Section.\n",
    "\n",
    "### Contents\n",
    "\n",
    "[2.1 Utility Functions](#utility)\n",
    "\n",
    "[2.2 Derivative of a Pure Rotation](#dR)\n",
    "* Deriving the Manipulator Jacobian\n",
    "* First Derivative of an Elementary Transform\n",
    "  * Derivative of a Pure Rotation\n",
    "\n",
    "[2.3 Derivative of a Pure Translation](#dt)\n",
    "* Deriving the Manipulator Jacobian\n",
    "* First Derivative of an Elementary Transform\n",
    "  * Derivative of a Pure Translation\n",
    "\n",
    "[2.4 The Manipulator Jacobian](#J)\n",
    "* The Manipulator Jacobian\n",
    "\n",
    "[2.5 Fast Manipulator Jacobian](#Jf)\n",
    "* Fast Manipulator Jacobian\n",
    "\n",
    "[2.6 Changing Jacobian Frame](#frame)\n",
    "* Deriving the Manipulator Jacobian\n",
    "\n",
    "[2.7 Robotics Toolbox Jacobians](#rtb)\n",
    "\n",
    "[2.8 Speed Analysis](#fast)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We will do the imports required for this notebook here\n",
    "\n",
    "# numpy provides import array and linear algebra utilities\n",
    "import numpy as np\n",
    "\n",
    "# the robotics toolbox provides robotics specific functionality\n",
    "import roboticstoolbox as rtb\n",
    "\n",
    "# spatial math provides objects for representing transformations\n",
    "import spatialmath as sm\n",
    "import spatialmath.base as smb\n",
    "\n",
    "# use timeit to benchmark some methods\n",
    "from timeit import default_timer as timer\n",
    "\n",
    "# ansitable is a great package for printing tables in a terminal\n",
    "from ansitable import ANSITable"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='utility'></a>\n",
    "\n",
    "### 2.1 Utility Functions\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "In this section we will use operations on skew symmetric and augmented skew-symmetric matrices\n",
    "\n",
    "<img src=\"img/skew_dark.png\" alt=\"drawing\" width=\"1300\"/>\n",
    "\n",
    "_Shown on the left is a vector $\\bf{s}\\in \\mathbb{R}^3$ along with its corresponding skew symmetric matrix $\\bf{S} \\in \\bf{so}(3) \\subset \\mathbb{R}^{3 \\times 3}$. Shown on the right is a vector $\\bf{\\hat{s}}\\in \\mathbb{R}^6$ along with its corresponding augmented skew symmetric matrix $\\bf{\\hat{S}} \\in \\bf{se}(3) \\subset \\mathbb{R}^{4 \\times 4}$. The skew functions $[\\cdot]_\\times : \\mathbb{R}^3 \\mapsto \\bf{so}(3)$ maps a vector to a skew symmetric matrix, and $[\\cdot] : \\mathbb{R}^6 \\mapsto \\bf{se}(3)$ maps a vector to an augmented skew symmetric matrix. The inverse skew functions $\\mathsf{V}_\\times(\\cdot) : \\bf{so}(3) \\mapsto \\mathbb{R}^3$ maps a skew symmetric matrix to a vector and $\\mathsf{V}(\\cdot) : \\bf{se}(3) \\mapsto \\mathbb{R}^6$ maps an augmented skew symmetric matrix to a vector._\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "# We can program these in Python\n",
    "# These methods assume the inputs are correctly sized and numpy arrays\n",
    "\n",
    "def vex(mat):\n",
    "    '''\n",
    "    Converts a 3x3 skew symmetric matric to a 3 vector\n",
    "    '''\n",
    "\n",
    "    return np.array([mat[2, 1], mat[0, 2], mat[1, 0]])\n",
    "\n",
    "def skew(vec):\n",
    "    '''\n",
    "    Converts a 3 vector to a 3x3 skew symmetric matrix\n",
    "    '''\n",
    "\n",
    "    return np.array([\n",
    "        [0, -vec[2], vec[1]],\n",
    "        [vec[2], 0, -vec[0]],\n",
    "        [-vec[1], vec[0], 0]\n",
    "    ])\n",
    "\n",
    "\n",
    "def vexa(mat):\n",
    "    '''\n",
    "    Converts a 4x4 augmented skew symmetric matric to a 6 vector\n",
    "    '''\n",
    "\n",
    "    return np.array([mat[0, 3], mat[1, 3], mat[2, 3], mat[2, 1], mat[0, 2], mat[1, 0]])\n",
    "\n",
    "def skewa(vec):\n",
    "    '''\n",
    "    Converts a 6 vector to a 4x4 augmented skew symmetric matrix\n",
    "    '''\n",
    "\n",
    "    return np.array([\n",
    "        [0, -vec[5], vec[4], vec[0]],\n",
    "        [vec[5], 0, -vec[3], vec[1]],\n",
    "        [-vec[4], vec[3], 0, vec[2]],\n",
    "        [0, 0, 0, 0]\n",
    "    ])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "x: \n",
      "[1 2 3]\n",
      "skew form of x: \n",
      "[[ 0 -3  2]\n",
      " [ 3  0 -1]\n",
      " [-2  1  0]]\n",
      "Perform vex operation on that, we should get the original a vector back: \n",
      "[1 2 3]\n"
     ]
    }
   ],
   "source": [
    "# Test our skew and vex methods out\n",
    "\n",
    "x = np.array([1, 2, 3])\n",
    "\n",
    "sk_x = skew(x)\n",
    "\n",
    "print(f\"x: \\n{x}\")\n",
    "print(f\"skew form of x: \\n{sk_x}\")\n",
    "print(f\"Perform vex operation on that, we should get the original a vector back: \\n{vex(sk_x)}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "y: \n",
      "[1 2 3 4 5 6]\n",
      "augmented skew form of y: \n",
      "[[ 0 -6  5  1]\n",
      " [ 6  0 -4  2]\n",
      " [-5  4  0  3]\n",
      " [ 0  0  0  0]]\n",
      "Perform vexa operation on that, we should get the original y vector back: \n",
      "[1 2 3 4 5 6]\n"
     ]
    }
   ],
   "source": [
    "y = np.array([1, 2, 3, 4, 5, 6])\n",
    "\n",
    "ska_y = skewa(y)\n",
    "\n",
    "print(f\"y: \\n{y}\")\n",
    "print(f\"augmented skew form of y: \\n{ska_y}\")\n",
    "print(f\"Perform vexa operation on that, we should get the original y vector back: \\n{vexa(ska_y)}\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "We also need a function to extract the translational or rotational component from an $\\bf{SE}(3)$ or augmented skew-symmetric matrix\n",
    "\n",
    "<br>\n",
    "\n",
    "<br>\n",
    "\n",
    "<img src=\"img/transform_dark.png\" alt=\"drawing\" width=\"700\"/>\n",
    "\n",
    "\n",
    "_Visualisation of a homogeneous transformation matrix (the derivatives share the form of $\\ \\bf{T}$, except will have a 0 instead of a 1 located at $\\bf{T}_{44}$). Where the matrix $\\rho(\\bf{T}) \\in \\mathbb{R}^{3 \\times 3}$ of green boxes forms the rotation component, and the vector $\\tau(\\bf{T}) \\in \\mathbb{R}^{3}$ of blue boxes form the translation component. The rotation component can be extracted through the function $\\rho(\\cdot) : \\mathbb{R}^{4\\times 4} \\mapsto  \\mathbb{R}^{3\\times 3}$, while the translation component can be extracted through the function $\\tau(\\cdot) : \\mathbb{R}^{4\\times 4} \\mapsto  \\mathbb{R}^{3}$._\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def ρ(tf):\n",
    "    '''\n",
    "    The method extracts the rotational component from an SE3\n",
    "    '''\n",
    "    return tf[:3, :3]\n",
    "\n",
    "def τ(tf):\n",
    "    '''\n",
    "    The method extracts the translation component from an SE3\n",
    "    '''\n",
    "    return tf[:3, 3]"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='dR'></a>\n",
    "### 2.2 Derivative of a Pure Rotation\n",
    "---\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "The derivative of a rotation matrix with respect to the rotation angle $\\theta$ is required when considering a revolute joint and\n",
    "can be shown to be\n",
    "\n",
    "\\begin{align*}\n",
    "    \\frac{\\mathrm{d}\\bf{R}(\\theta)}\n",
    "         {\\mathrm{d} \\theta} \n",
    "    &= {[\\bf{\\hat{\\omega}}]_\\times} \\ \\bf{R} \\big( \\theta(t) \\big)\n",
    "\\end{align*}\n",
    "\n",
    "where the unit vector $\\bf{\\hat{\\omega}}$ is the joint rotation axis.\n",
    "\n",
    "The rotation axis $\\bf{\\hat{\\omega}}$ can be recovered using the inverse skew operator\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{\\hat{\\omega}} &=\n",
    "    \\mathsf{V}_\\times\n",
    "    \\left(\n",
    "    \\frac{\\mathrm{d} \\bf{R}(\\theta)}\n",
    "         {\\mathrm{d} \\theta}\n",
    "    \\bf{R}  \\big( \\theta(t)\\big)^\\top\n",
    "    \\right)\n",
    "\\end{align*}\n",
    "\n",
    "since $ \\bf{R} \\in \\bf{SO}(3)$, then $\\bf{R}^{-1} = \\bf{R}^\\top$.\n",
    "\n",
    "For an ETS, we only need to consider the elementary rotations $\\bf{R}_x$, $\\bf{R}_y$, and $\\bf{R}_z$ which are embedded within $\\bf{SE}(3)$, as $\\bf{T}_{R_{x}}$, $\\bf{T}_{R_{y}}$, and $\\bf{T}_{R_{z}}$ i.e. pure rotations with no translational component. We can show that the derivative of each elementary rotation with respect to a rotation angle is\n",
    "\n",
    "\\begin{align*}\n",
    "    \\dfrac{\\mathrm{d} \\bf{T}_{R_{x}}(\\theta)}\n",
    "          {\\mathrm{d} \\theta}\n",
    "    &=     \n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & -1 & 0 \\\\\n",
    "        0 & 1 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \n",
    "    \\end{pmatrix} \\bf{T}_{R_{x}}(\\theta) = \\big[ \\bf{\\hat{R}}_x \\big] \\bf{T}_{R_{x}}(\\theta) \\\\\n",
    "    \\dfrac{\\mathrm{d} \\bf{T}_{R_{y}}(\\theta)}\n",
    "          {\\mathrm{d} \\theta}\n",
    "    &=     \n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 1 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        -1 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \n",
    "    \\end{pmatrix} \\bf{T}_{R_{y}}(\\theta) = \\big[ \\bf{\\hat{R}}_y \\big] \\bf{T}_{R_{y}}(\\theta) \\\\\n",
    "    \\dfrac{\\mathrm{d} \\bf{T}_{R_{z}}(\\theta)}\n",
    "          {\\mathrm{d} \\theta}\n",
    "    &=     \n",
    "    \\begin{pmatrix}\n",
    "        0 & -1 & 0 & 0 \\\\\n",
    "        1 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0  \n",
    "    \\end{pmatrix} \\bf{T}_{R_{z}}(\\theta) = \\big[ \\bf{\\hat{R}}_z \\big] \\bf{T}_{R_{z}}(\\theta)\n",
    "\\end{align*}\n",
    "\n",
    "where each of the augmented skew-symmetric matrices $[\\bf{\\hat{R}} ]$ above corresponds to one of the generators of $\\bf{SE}(3)$ which lies in $\\bf{se}(3)$, the tangent space of $\\bf{SE}(3)$.\n",
    "If a joints defined positive rotation is a negative rotation about the axis, , then $\\bf{T}_{\\bf{R}_i}(\\theta)^\\top$ is used to calculate the derivative.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We can make Python functions which perform these derivatives\n",
    "\n",
    "def dTRx(θ, flip):\n",
    "    '''\n",
    "    Calculates the derivative of an SE3 which is a pure rotation around\n",
    "    the x-axis by amount θ\n",
    "    '''\n",
    "    # This is the [Rhat_x] matrix in the maths above\n",
    "    Rhx = np.array([\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, -1, 0],\n",
    "        [0, 1, 0, 0],\n",
    "        [0, 0, 0, 0]\n",
    "    ])\n",
    "\n",
    "    # This is the T_R_x matrix in the maths above\n",
    "    Trx = smb.trotx(θ)\n",
    "\n",
    "    if flip:\n",
    "        return Rhx.T @ Trx.T\n",
    "    else:\n",
    "        return Rhx @ Trx\n",
    "\n",
    "def dTRy(θ, flip):\n",
    "    '''\n",
    "    Calculates the derivative of an SE3 which is a pure rotation around\n",
    "    the y-axis by amount θ\n",
    "    '''\n",
    "    # This is the [Rhat_y] matrix in the maths above\n",
    "    Rhy = np.array([\n",
    "        [0, 0, 1, 0],\n",
    "        [0, 0, 0, 0],\n",
    "        [-1, 0, 0, 0],\n",
    "        [0, 0, 0, 0]\n",
    "    ])\n",
    "\n",
    "    # This is the T_R_y matrix in the maths above\n",
    "    Try = smb.troty(θ)\n",
    "\n",
    "    if flip:\n",
    "        return Rhy.T @ Try.T\n",
    "    else:\n",
    "        return Rhy @ Try\n",
    "\n",
    "def dTRz(θ, flip):\n",
    "    '''\n",
    "    Calculates the derivative of an SE3 which is a pure rotation around\n",
    "    the z-axis by amount θ\n",
    "    '''\n",
    "    # This is the [Rhat_z] matrix in the maths above\n",
    "    Rhz = np.array([\n",
    "        [0, -1, 0, 0],\n",
    "        [1, 0, 0, 0],\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, 0, 0]\n",
    "    ])\n",
    "\n",
    "    # This is the T_R_z matrix in the maths above\n",
    "    Trz = smb.trotz(θ)\n",
    "\n",
    "    if flip:\n",
    "        return Rhz.T @ Trz.T\n",
    "    else:\n",
    "        return Rhz @ Trz\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='dt'></a>\n",
    "### 2.3 Derivative of a Pure Translation\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "The derivative of a homogeneous transformation matrix with respect to translation is required when considering a prismatic joint. For an ETS, these translations are embedded in $\\bf{SE}(3)$ as $\\bf{T}_{t_x}$, $\\bf{T}_{t_y}$, and $\\bf{T}_{t_z}$ which are pure translations with zero rotational component. We can show that the derivative of each elementary translation with respect to a translation is \n",
    "\n",
    "\\begin{align*}\n",
    "    \\dfrac{\\mathrm{d} \\bf{T}_{t_x}(d)}\n",
    "          {\\mathrm{d} d}\n",
    "    &=     \n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 0 & 1 \\\\\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \n",
    "    \\end{pmatrix} = [\\bf{\\hat{t}}_x], \\\\\n",
    "    \\dfrac{\\mathrm{d} \\bf{T}_{t_y}(d)}\n",
    "          {\\mathrm{d} d}\n",
    "    &=     \n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 1 \\\\\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \n",
    "    \\end{pmatrix} = [\\bf{\\hat{t}}_y], \\\\\n",
    "    \\dfrac{\\mathrm{d} \\bf{T}_{t_z}(d)}\n",
    "          {\\mathrm{d} d}\n",
    "    &=     \n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 0 \\\\\n",
    "        0 & 0 & 0 & 1 \\\\\n",
    "        0 & 0 & 0 & 0 \n",
    "    \\end{pmatrix} = [\\bf{\\hat{t}}_z],\n",
    "\\end{align*}\n",
    "\n",
    "where each of the augmented skew symmetric matrices $[ \\, \\bf{\\hat{t}} \\, ]$ above are the remaining three generators of $\\bf{SE}(3)$ which lie in $\\bf{se}(3)$.\n",
    "If a joints defined positive translation is a negative translation along the axis, \n",
    "then $-[ \\, \\bf{\\hat{t}} \\, ]$ should be used to calculate the derivative.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We can make Python functions which perform these derivatives\n",
    "# Since these are constant, we don't need to handle joints being\n",
    "# flipped like we do with revolute joints\n",
    "\n",
    "def dTtx():\n",
    "    '''\n",
    "    Calculates the derivative of an SE3 which is a pure translation along\n",
    "    the x-axis by amount d\n",
    "    '''\n",
    "    # This is the [That_x] matrix in the maths above\n",
    "    Thx = np.array([\n",
    "        [0, 0, 0, 1],\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, 0, 0]\n",
    "    ])\n",
    "\n",
    "    return Thx\n",
    "\n",
    "def dTty():\n",
    "    '''\n",
    "    Calculates the derivative of an SE3 which is a pure translation along\n",
    "    the y-axis by amount d\n",
    "    '''\n",
    "    # This is the [That_y] matrix in the maths above\n",
    "    Thy = np.array([\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, 0, 1],\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, 0, 0]\n",
    "    ])\n",
    "\n",
    "    return Thy\n",
    "\n",
    "def dTtz():\n",
    "    '''\n",
    "    Calculates the derivative of an SE3 which is a pure translation along\n",
    "    the z-axis by amount d\n",
    "    '''\n",
    "    # This is the [That_z] matrix in the maths above\n",
    "    Thz = np.array([\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, 0, 0],\n",
    "        [0, 0, 0, 1],\n",
    "        [0, 0, 0, 0]\n",
    "    ])\n",
    "\n",
    "    return Thz"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='J'></a>\n",
    "### 2.4 The Manipulator Jacobian\n",
    "---\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Before going any further, lets remake the panda model we made in the first notebook\n",
    "E1 = rtb.ET.tz(0.333) \n",
    "E2 = rtb.ET.Rz()\n",
    "E3 = rtb.ET.Ry() \n",
    "E4 = rtb.ET.tz(0.316) \n",
    "E5 = rtb.ET.Rz()\n",
    "E6 = rtb.ET.tx(0.0825) \n",
    "E7 = rtb.ET.Ry(flip=True) \n",
    "E8 = rtb.ET.tx(-0.0825) \n",
    "E9 = rtb.ET.tz(0.384) \n",
    "E10 = rtb.ET.Rz()\n",
    "E11 = rtb.ET.Ry(flip=True)\n",
    "E12 = rtb.ET.tx(0.088) \n",
    "E13 = rtb.ET.Rx(np.pi / 2) \n",
    "E14 = rtb.ET.tz(0.107)\n",
    "E15 = rtb.ET.Rz()\n",
    "panda = E1 * E2 * E3 * E4 * E5 * E6 * E7 * E8 * E9 * E10 * E11 * E12 * E13 * E14 * E15\n",
    "\n",
    "# And make a joint coordinate array q\n",
    "q = np.array([0, -0.3, 0, -2.2, 0, 2, 0.79])"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "Now we can calculate the derivative of an ETS. To find out how the $j^{th}$ joint affects the end-effector pose, apply the chain rule to (1 in paper)\n",
    "\n",
    "\\begin{align*}\n",
    "    \\frac{\\partial \\bf{T}(\\bf{q})}\n",
    "         {\\partial q_j} \n",
    "    &=\n",
    "    \\frac{\\partial} \n",
    "         {\\partial q_j} \n",
    "    \\left(\n",
    "        \\bf{E}_1(\\eta_1) \\bf{E}_2(\\eta_2) \\cdot\\cdot\\cdot \\bf{E}_M(\\eta_M) \n",
    "    \\right)\\\\\n",
    "    &= \n",
    "    \\prod_{i=1}^{\\mu(j)-1} \\!\\!\\!\\bf{E}_i(\\eta_i) \n",
    "    \\ \\ \n",
    "    \\frac{\\mathrm{d} \\bf{E}_{\\mu(j)}(q_j)} \n",
    "         {\\mathrm{d} q_j}\n",
    "    \\prod_{i=\\mu(j)+1}^{M} \\!\\!\\!\\!\\!\\!\\bf{E}_i(\\eta_i).\n",
    "\\end{align*}\n",
    "\n",
    "The derivative of the elementary transform with respect to a joint coordinate above is obtained using one of the functions in 2.2 for a revolute joint, or 2.3 for a prismatic joint.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "def dET(et, η):\n",
    "    '''\n",
    "    This method takes an ET and returns the derivative with respect to the joint coordinate\n",
    "    This is here for convenience\n",
    "    '''\n",
    "\n",
    "    if et.axis == 'Rx':\n",
    "        return dTRx(η, et.isflip)\n",
    "    elif et.axis == 'Ry':\n",
    "        return dTRy(η, et.isflip)\n",
    "    elif et.axis == 'Rz':\n",
    "        return dTRz(η, et.isflip)\n",
    "    elif et.axis == 'tx':\n",
    "        return dTtx()\n",
    "    elif et.axis == 'ty':\n",
    "        return dTty()\n",
    "    else:\n",
    "        return dTtz()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "def dT_j(ets, j, q):\n",
    "    '''\n",
    "    This methods calculates the dervative of an SE3 with respect to joint coordinate j\n",
    "    '''\n",
    "\n",
    "    # Allocate an identity matrix for the result\n",
    "    dT = np.eye(4)\n",
    "\n",
    "    # Find the jth variable et in the ets\n",
    "    et_j = ets.joints()[j]\n",
    "\n",
    "    # Loop overs the ETs in the robot ETS\n",
    "    for et in ets:\n",
    "        if et == et_j:\n",
    "            # This ET is variable and corresponds to joint j\n",
    "            dT = dT @ dET(et, q[et.jindex])\n",
    "        elif et.isjoint:\n",
    "            # This ET is a variable joint\n",
    "            # Use the q array to specify the joint angle for the variable ET\n",
    "            dT = dT @ et.A(q[et.jindex])\n",
    "        else:\n",
    "            # This ET is static\n",
    "            dT = dT @ et.A()\n",
    "\n",
    "    return dT"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "\n",
    "We can form the angular velocity component of the $j^{th}$ column of the manipulator Jacobian\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{J}_{\\omega_j}(\\bf{q}) \n",
    "    &=\n",
    "    \\mathsf{V}_\\times\n",
    "    \\left(\n",
    "        \\rho\n",
    "        \\left(\n",
    "            \\frac{\\partial \\bf{T}(\\bf{q})}\n",
    "                {\\partial q_j} \n",
    "        \\right)\n",
    "        \\rho\n",
    "        \\left(\n",
    "            \\bf{T}(\\bf{q})\n",
    "        \\right)^\\top\n",
    "    \\right)\n",
    "\\end{align*}\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Jω_j(ets, j, q):\n",
    "    '''\n",
    "    This method calculates the rotational component of the jth column of the manipulator Jacobian\n",
    "    '''\n",
    "\n",
    "    # Calculate the forward kinematics at q\n",
    "    T = ets.eval(q)\n",
    "\n",
    "    # Calculate the derivative of T with respect to q_j\n",
    "    dT = dT_j(ets, j, q)\n",
    "\n",
    "    Jω = vex(ρ(dT) @ (ρ(T).T))\n",
    "\n",
    "    return Jω\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[-2.95520207e-01 -6.22199256e-33  9.55336489e-01]\n"
     ]
    }
   ],
   "source": [
    "# Calculate the angular component of the third column of Panda manipulator Jacobian\n",
    "print(Jω_j(panda, 2, q))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "We can form the translational velocity component of the $j^{th}$ column of the manipulator Jacobian using\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{J}_{v_j}(\\bf{q}) \n",
    "    &=\n",
    "    \\tau\n",
    "    \\left(\n",
    "        \\frac{\\partial \\bf{T}(\\bf{q})}\n",
    "            {\\partial q_j} \n",
    "    \\right).\n",
    "\\end{align*}\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Jv_j(ets, j, q):\n",
    "    '''\n",
    "    This method calculates the tranlation component of the jth column of the manipulator Jacobian\n",
    "    '''\n",
    "\n",
    "    # Calculate the derivative of T with respect to q_j\n",
    "    dT = dT_j(ets, j, q)\n",
    "\n",
    "    Jv = τ(dT)\n",
    "\n",
    "    return Jv\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.0372881  0.         0.47761099]\n"
     ]
    }
   ],
   "source": [
    "# Calculate the translational component of the third column of Panda manipulator Jacobian\n",
    "print(Jv_j(panda, 3, q))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "Stacking the translational and angular velocity components, the $j^{th}$ column of the manipulator Jacobian becomes \n",
    "\n",
    "\\begin{equation*}\n",
    "     \\bf{J}_j(\\bf{q}) = \n",
    "     \\begin{pmatrix} \n",
    "          \\bf{J}_{v_j}(\\bf{q}) \\\\ \n",
    "          \\bf{J}_{\\omega_j}(\\bf{q}) \n",
    "     \\end{pmatrix} \\in \\mathbb{R}^{6}\n",
    "\\end{equation*}\n",
    "\n",
    "where the full manipulator Jacobian is \n",
    "\n",
    "\\begin{equation*}\n",
    "    \\bf{J}(\\bf{q}) = \n",
    "    \\begin{pmatrix} \n",
    "        \\bf{J}_1(\\bf{q}) & \\cdots & \\bf{J}_n(\\bf{q})\n",
    "    \\end{pmatrix} \\in \\mathbb{R}^{6 \\times n}.\n",
    "\\end{equation*}\n",
    "\n",
    "<br>\n",
    "\n",
    "<br>\n",
    "\n",
    "<img src=\"img/jacobian_dark.png\" alt=\"drawing\" width=\"500\"/>\n",
    "\n",
    "_Visualisation of the Jacobian $\\bf{J}(\\bf{q})$ of a 7-jointed manipulator. Each column describes how the end-effector pose changes due to motion of the corresponding joint. The top three rows $\\bf{J}_v$  correspond to the linear velocity of the end-effector while the bottom three rows $\\bf{J}_\\omega$ correspond to the angular velocity of the end-effector._\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "# By using our previously defined methods, we can now calculate the manipulator Jacobian\n",
    "\n",
    "def jacob0(ets, q):\n",
    "    '''\n",
    "    This method calculates the manipulator Jacobian in the world frame\n",
    "    '''\n",
    "\n",
    "    # Allocate array for the Jacobian\n",
    "    # It is a 6xn matrix\n",
    "    J = np.zeros((6, ets.n))\n",
    "\n",
    "    for j in range(ets.n):\n",
    "        Jv = Jv_j(ets, j, q)\n",
    "        Jω = Jω_j(ets, j, q)\n",
    "\n",
    "        J[:3, j] = Jv\n",
    "        J[3:, j] = Jω\n",
    "\n",
    "    return J"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The manipulator Jacobian (world-frame) \n",
      "in configuration q = [ 0.   -0.3   0.   -2.2   0.    2.    0.79] \n",
      "is: \n",
      "[[ 0.11  0.29  0.1   0.04 -0.03 -0.01  0.  ]\n",
      " [ 0.46  0.    0.53  0.   -0.04  0.    0.  ]\n",
      " [ 0.   -0.46  0.03  0.48 -0.1   0.09  0.  ]\n",
      " [ 0.   -0.   -0.3   0.    0.95  0.   -0.  ]\n",
      " [ 0.    1.   -0.   -1.   -0.   -1.   -1.  ]\n",
      " [ 1.    0.    0.96  0.   -0.32  0.    0.  ]]\n"
     ]
    }
   ],
   "source": [
    "# Calculate the manipulator Jacobian of the Panda in the world frame\n",
    "J = jacob0(panda, q)\n",
    "\n",
    "print(f\"The manipulator Jacobian (world-frame) \\nin configuration q = {q} \\nis: \\n{np.round(J, 2)}\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "   \n",
    "<a id='Jf'></a>\n",
    "### 2.5 Fast Manipulator Jacobian\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "<img src=\"img/rotation_dark.png\" alt=\"drawing\" width=\"500\"/>\n",
    "\n",
    "_The two vector representation of a rotation matrix $\\R \\in \\bf{SO}(3)$. The rotation matrix $\\mathbb{R}$ describes the coordinate frame in terms of three orthogonal vectors $\\bf{\\hat{n}}$, $\\bf{\\hat{o}}$, and $\\bf{\\hat{a}}$ which are the axes of the rotated frame expressed in the reference coordinate frame $\\bf{\\hat{x}}$, $\\bf{\\hat{y}}$, and $\\bf{\\hat{z}}$. As shown above, each of the vectors $\\bf{\\hat{n}}$, $\\bf{\\hat{o}}$, and $\\bf{\\hat{a}}$ can be calculated using the cross product of the other two._\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "Calculating the manipulator Jacobian using the method in 2.4 is easy to understand, but has $\\mathcal{O}(n^2)$ time complexity -- we can do better.\n",
    "\n",
    "\n",
    "Expanding the above equations (26) and (27), and simplifying using $\\bf{R} \\bf{R}^\\top = \\bf{1}$ gives\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{J}_{\\omega_j}(\\bf{q}) \n",
    "    &=\n",
    "    \\mathsf{V}_\\times \\bigg(\n",
    "        \\rho\n",
    "        \\left(\n",
    "            ^0\\bf{T}_j\n",
    "        \\right)\n",
    "        \\rho\n",
    "        \\left(\n",
    "        [\\bf{\\hat{G}}_{\\mu(j)}]\n",
    "        \\right)\n",
    "        \\left(\n",
    "            \\rho(^0\\bf{T}_j)^\\top\n",
    "        \\right)\n",
    "    \\bigg)\n",
    "\\end{align*}\n",
    "\n",
    "where $^0\\bf{T}_j$ represents the transform from the base frame to joint $j$, and $[\\bf{\\hat{G}}_{\\mu(j)}]$ corresponds to one of the 6 generators from equations in 2.4 or (20 - 25) in the paper. \n",
    "\n",
    "In the case of a prismatic joint, $\\rho(\\bf{\\hat{G}}_{\\mu(j)})$ will be a $3 \\times 3$ matrix of zeros which results in zero angular velocity. In the case of a revolute joint, the angular velocity is parallel to the axis of joint rotation.\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{J}_{\\omega_j}(\\bf{q}) \n",
    "    &=\n",
    "    \\left\\{\n",
    "    \\begin{matrix}\n",
    "        \\bf{\\hat{n}}_j & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{\\bf{R}_x}\\\\\n",
    "        \\bf{\\hat{o}}_j & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{\\bf{R}_y}\\\\\n",
    "        \\bf{\\hat{a}}_j & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{\\bf{R}_z}\\\\\n",
    "        (0, \\ 0, \\ 0)^\\top & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{t}\n",
    "    \\end{matrix}\n",
    "    \\right.\n",
    "\\end{align*}\n",
    "\n",
    "where $\\begin{pmatrix} \\bf{\\hat{n}}_j & \\bf{\\hat{o}}_j & \\bf{\\hat{a}}_j \\end{pmatrix} = \\rho(^0\\bf{T}_j)$ are the columns of a rotation matrix as shown in the Figure above.\n",
    "\n",
    "\n",
    "Expanding (28) using (26) provides\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{J}_{v_j}(\\bf{q})\n",
    "    &=\n",
    "        \\tau\n",
    "        \\left(\n",
    "            ^0\\bf{T}_j\n",
    "            [\\bf{\\hat{G}}_{\\mu(j)}]\n",
    "            {^j\\bf{T}}_{e}\n",
    "        \\right)\n",
    "\\end{align*}\n",
    "\n",
    "which reduces to \n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{J}_{v_j}(\\bf{q}) \n",
    "    &=\n",
    "    \\left\\{ \n",
    "    \\begin{matrix}\n",
    "        \\bf{\\hat{a}}_j y_e - \\bf{\\hat{o}}_j z_e & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{\\bf{R}_x}\\\\\n",
    "        \\bf{\\hat{n}}_j z_e - \\bf{\\hat{a}}_j x_e & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{\\bf{R}_y}\\\\\n",
    "        \\bf{\\hat{o}}_j x_e - \\bf{\\hat{y}}_j y_e & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{\\bf{R}_z}\\\\\n",
    "        \\bf{\\hat{n}}_j                      & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{t_x}\\\\\n",
    "        \\bf{\\hat{o}}_j                      & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{t_y}\\\\\n",
    "        \\bf{\\hat{a}}_j                      & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{t_z}\\\\\n",
    "    \\end{matrix}\n",
    "    \\right.\n",
    "\\end{align*}\n",
    "\n",
    "where\n",
    "$\\begin{pmatrix}x_e & y_e & z_e \\end{pmatrix}^\\top = \\tau({^j\\bf{T}}_{e})$.\n",
    "\n",
    "This simplification reduces the time complexity of computation of the manipulator Jacobian to $\\mathcal{O}(n)$.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Now making the efficient version in Python\n",
    "\n",
    "def jacob0_efficient(ets, q):\n",
    "    '''\n",
    "    This method calculates the manipulator Jacobian in the world frame using a more efficient method\n",
    "    '''\n",
    "    T = ets.eval(q)\n",
    "\n",
    "    U = np.eye(4)\n",
    "    j = 0\n",
    "    J = np.zeros((6, ets.n))\n",
    "\n",
    "    for ets in ets:\n",
    "        jindex = ets.jindex\n",
    "\n",
    "        if ets.isjoint:\n",
    "            U = U @ ets.A(q[jindex])  # type: ignore\n",
    "\n",
    "            Tu = sm.SE3(U, check=False).inv().A @ T\n",
    "            n = U[:3, 0]\n",
    "            o = U[:3, 1]\n",
    "            a = U[:3, 2]\n",
    "            x = Tu[0, 3]\n",
    "            y = Tu[1, 3]\n",
    "            z = Tu[2, 3]\n",
    "\n",
    "            if ets.isflip:\n",
    "                sign = -1.0\n",
    "            else:\n",
    "                sign = 1.0\n",
    "\n",
    "            if ets.axis == \"Rz\":\n",
    "                J[:3, j] = sign * ((o * x) - (n * y))\n",
    "                J[3:, j] = sign * a\n",
    "\n",
    "            elif ets.axis == \"Ry\":\n",
    "                J[:3, j] = sign * ((n * z) - (a * x))\n",
    "                J[3:, j] = sign * o\n",
    "\n",
    "            elif ets.axis == \"Rx\":\n",
    "                J[:3, j] = sign * ((a * y) - (o * z))\n",
    "                J[3:, j] = sign * n\n",
    "\n",
    "            elif ets.axis == \"tx\":\n",
    "                J[:3, j] = n\n",
    "                J[3:, j] = np.array([0, 0, 0])\n",
    "\n",
    "            elif ets.axis == \"ty\":\n",
    "                J[:3, j] = o\n",
    "                J[3:, j] = np.array([0, 0, 0])\n",
    "\n",
    "            elif ets.axis == \"tz\":\n",
    "                J[:3, j] = a\n",
    "                J[3:, j] = np.array([0, 0, 0])\n",
    "\n",
    "            j += 1\n",
    "        else:\n",
    "            U = U @ ets.A()\n",
    "\n",
    "    return J"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The manipulator Jacobian (world-frame) \n",
      "in configuration q = [ 0.   -0.3   0.   -2.2   0.    2.    0.79] \n",
      "is: \n",
      "[[ 0.11  0.29  0.1   0.04 -0.03 -0.01 -0.  ]\n",
      " [ 0.46  0.    0.53 -0.   -0.04 -0.    0.  ]\n",
      " [ 0.   -0.46  0.03  0.48 -0.1   0.09  0.  ]\n",
      " [ 0.    0.   -0.3  -0.    0.95 -0.   -0.  ]\n",
      " [ 0.    1.    0.   -1.    0.   -1.   -1.  ]\n",
      " [ 1.    0.    0.96 -0.   -0.32 -0.    0.  ]]\n"
     ]
    }
   ],
   "source": [
    "J = jacob0_efficient(panda, q)\n",
    "\n",
    "print(f\"The manipulator Jacobian (world-frame) \\nin configuration q = {q} \\nis: \\n{np.round(J, 2)}\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='frame'></a>\n",
    "### 2.6 Changing Jacobian Frame\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "We have been calculating the manipulator Jacobian in the world-frame\n",
    "\n",
    "\\begin{equation*}\n",
    "     {^0\\bf{J}}(\\bf{q}).\n",
    "\\end{equation*}\n",
    "\n",
    "is the manipulator Jacobian matrix expressed in the world-coordinate frame.\n",
    "\n",
    "The Jacobian expressed in the end-effector frame is\n",
    "\n",
    "\\begin{equation*}\n",
    "    {^e\\bf{J}}(\\bf{q}) =\n",
    "    \\begin{pmatrix} \n",
    "        {{^0\\bf{R}}_e^\\top} & \\bf{0} \\\\ \n",
    "        \\bf{0} & {{^0\\bf{R}}_e^\\top}\n",
    "     \\end{pmatrix}\n",
    "     {^0\\bf{J}}(\\bf{q})\n",
    "\\end{equation*}\n",
    "\n",
    "where ${^0\\bf{R}}_e$ is a rotation matrix representing the orientation of the end-effector in the world frame.\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Now making it in Python\n",
    "\n",
    "def jacobe(ets, q):\n",
    "    '''\n",
    "    This method calculates the manipulator Jacobian in the end-effector frame\n",
    "    '''\n",
    "\n",
    "    # Calculate world-frame Jacobian\n",
    "    J0 = jacob0_efficient(ets, q)\n",
    "\n",
    "    # Calculate the forward kineamtics\n",
    "    T = ets.eval(q)\n",
    "\n",
    "    # Extract rotation from T and transpose\n",
    "    R = T[:3, :3].T\n",
    "\n",
    "    # Make our velocity transform matrix\n",
    "    tr = np.zeros((6, 6))\n",
    "\n",
    "    # Put the rotation in the top left and bottom right of the matrix\n",
    "    tr[:3, :3] = R\n",
    "    tr[3:, 3:] = R\n",
    "\n",
    "    # Perform the matrix multiplication to calculate the Jacobian\n",
    "    # in the end-effector frame\n",
    "    Je = tr @ J0\n",
    "\n",
    "    return Je\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The manipulator Jacobian (end-effector) \n",
      "in configuration q = [ 0.   -0.3   0.   -2.2   0.    2.    0.79] \n",
      "is: \n",
      "[[ 0.07 -0.18  0.09  0.39 -0.1   0.06  0.  ]\n",
      " [-0.08 -0.52 -0.06  0.27 -0.04  0.06  0.  ]\n",
      " [-0.46 -0.   -0.53  0.    0.04  0.    0.  ]\n",
      " [ 0.78  0.    0.56 -0.    0.34 -0.   -0.  ]\n",
      " [ 0.63  0.    0.83 -0.   -0.94 -0.    0.  ]\n",
      " [ 0.   -1.    0.    1.   -0.    1.    1.  ]]\n"
     ]
    }
   ],
   "source": [
    "# Calculate the manipulator Jacobian of the Panda in the end-effector frame\n",
    "J = jacobe(panda, q)\n",
    "\n",
    "print(f\"The manipulator Jacobian (end-effector) \\nin configuration q = {q} \\nis: \\n{np.round(J, 2)}\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='rtb'></a>\n",
    "### 2.7 Robotics Toolbox Jacobians\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "\n",
    "The `ETS` class within the Robotics Toolbox supports many kinematic methods (such as the previously discussed `.eval` and `.fkine` methods).\n",
    "\n",
    "The `.jacob0` and `.jacobe` methods in the `ETS` class calculate the world and end-effector frame Jacobians respectively.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The manipulator Jacobian (world frame) is: \n",
      "[[ 0.11  0.29  0.1   0.04 -0.03 -0.01  0.  ]\n",
      " [ 0.46  0.    0.53  0.   -0.04  0.    0.  ]\n",
      " [-0.   -0.46  0.03  0.48 -0.1   0.09  0.  ]\n",
      " [ 0.   -0.   -0.3   0.    0.95  0.   -0.  ]\n",
      " [ 0.    1.    0.   -1.    0.   -1.   -1.  ]\n",
      " [ 1.    0.    0.96  0.   -0.32  0.    0.  ]]\n",
      "\n",
      "The manipulator Jacobian (end-effector frame) is: \n",
      "[[ 0.07 -0.18  0.09  0.39 -0.1   0.06  0.  ]\n",
      " [-0.08 -0.52 -0.06  0.27 -0.04  0.06  0.  ]\n",
      " [-0.46 -0.   -0.53  0.    0.04  0.    0.  ]\n",
      " [ 0.78  0.    0.56 -0.    0.34 -0.    0.  ]\n",
      " [ 0.63  0.    0.83 -0.   -0.94 -0.    0.  ]\n",
      " [ 0.   -1.    0.    1.   -0.    1.    1.  ]]\n"
     ]
    }
   ],
   "source": [
    "# Note: The panda object which we have been using is an instance of the ETS class\n",
    "\n",
    "# Calculate the world frame Jacobian using the ETS class\n",
    "J0 = panda.jacob0(q)\n",
    "\n",
    "# Calculate the end-effector frame Jacobian using the ETS class\n",
    "Je = panda.jacobe(q)\n",
    "\n",
    "# View our Jacobians\n",
    "print(f\"The manipulator Jacobian (world frame) is: \\n{np.round(J0, 2)}\")\n",
    "print(f\"\\nThe manipulator Jacobian (end-effector frame) is: \\n{np.round(Je, 2)}\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='fast'></a>\n",
    "### 2.8 Speed Comparison\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "\n",
    "In this section we will briefly cover a speed comparison between the two `jacob0` methods shown here and the Jacobian implementation provided by the Robotics Toolbox.\n",
    "\n",
    "Kinematic methods in the toolbox are written using C extensions and wrapped by python methods. These C methods also have Python fallbacks to rely on if something goes wrong.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "Timing to Calculate Manipulator Jacobian over 1000 iterations\n",
      "\n",
      "┌─────────────────────┬─────────────────┬───────────────────┐\n",
      "│            Function │ Total Time (ms) │ Average Time (us) │\n",
      "├─────────────────────┼─────────────────┼───────────────────┤\n",
      "│          our jacob0\u001b[0m │        766.2759\u001b[0m │          766.2759\u001b[0m │\n",
      "│our jacob0_efficient\u001b[0m │         84.0204\u001b[0m │           84.0204\u001b[0m │\n",
      "│      toolbox jacob0\u001b[0m │          0.6925\u001b[0m │            0.6925\u001b[0m │\n",
      "└─────────────────────┴─────────────────┴───────────────────┘\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# Number of iterations to test over\n",
    "n = 1000\n",
    "\n",
    "# Make a random matrix of q values\n",
    "qt = np.random.random((n, 7))\n",
    "\n",
    "# Slow Jacobian implementation\n",
    "start = timer()\n",
    "for q in qt:\n",
    "    jacob0(panda, q)\n",
    "slow_time = timer() - start\n",
    "\n",
    "# Improved Jacobian implementation\n",
    "start = timer()\n",
    "for q in qt:\n",
    "    jacob0_efficient(panda, q)\n",
    "improved_time = timer() - start\n",
    "\n",
    "# Toolbox C improved Jacobian implementation\n",
    "start = timer()\n",
    "for q in qt:\n",
    "    panda.jacob0(q)\n",
    "rtb_time = timer() - start\n",
    "\n",
    "# Make a table\n",
    "print(f\"\\nTiming to Calculate Manipulator Jacobian over {n} iterations\\n\")\n",
    "table = ANSITable(\n",
    "    \"Function\",\n",
    "    \"Total Time (ms)\",\n",
    "    \"Average Time (us)\",\n",
    "    border=\"thin\",\n",
    ")\n",
    "\n",
    "table.row(\n",
    "    \"our jacob0\",\n",
    "    np.round(slow_time * 1000.0, 4),\n",
    "    np.round((slow_time / n) * 1000000.0, 4),\n",
    ")\n",
    "\n",
    "table.row(\n",
    "    \"our jacob0_efficient\",\n",
    "    np.round(improved_time * 1000.0, 4),\n",
    "    np.round((improved_time / n) * 1000000.0, 4),\n",
    ")\n",
    "\n",
    "table.row(\n",
    "    \"toolbox jacob0\",\n",
    "    np.round(rtb_time * 1000.0, 4),\n",
    "    np.round((rtb_time / n) * 1000000.0, 4),\n",
    ")\n",
    "table.print()\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "rtb",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.0"
  },
  "vscode": {
   "interpreter": {
    "hash": "4e8f2602a343ae62ff483b4df7805ab0eb447ccc1a56caf0631d5620d36b21ab"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
